/*
 * @Description: LIO
 * @Autor: Zijie Chen
 * @Date: 2023-12-28 11:09:59
 */

#ifndef LIO_H_
#define LIO_H_

#include <deque>
#include <numeric>

#include <sensor_msgs/Imu.h>

#include <glog/logging.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

#include <sophus/so3.hpp>

#include <pcl/common/transforms.h>

#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>

#include "sci_lio/faster_voxel_grid.h"
#include "sci_lio/point_type.h"
#include "sci_lio/utilities.hpp"
#include "sci_lio/voxel_map.h"
#include <bitset>

#include "sys/stat.h"
#include <igraph/igraph.h>

#include <opencv2/opencv.hpp>

//#include "clipperplus/clipperplus_clique.h"
#include "sci_lio/faster_voxel_grid.h"
#include "sci_lio/point_type.h"
using MyHashMap = tbb::concurrent_hash_map<size_t, std::shared_ptr<Voxel>>;
using MyAccessor = MyHashMap::accessor;
using namespace std;
/********yel add start********/
typedef struct
{
	int index;
	double score;
}Vote;

typedef struct
{
	int index;
	int degree;
	double score;
	std::vector<int> corre_index;
	int true_num;
}Vote_exp;

struct Correspondence {
  public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  Eigen::Vector3d mean_A = Eigen::Vector3d::Zero();
  Eigen::Vector3d mean_B = Eigen::Vector3d::Zero();
  Eigen::Matrix3d mahalanobis = Eigen::Matrix3d::Zero();
  Eigen::Vector4d plane_coeff = Eigen::Vector4d::Zero();
  //! 创新点：增加协方差矩阵的迹
  double tr_A;
  double tr_B;
  double tr_AB;
};

typedef struct
{
	int clique_index;
	int clique_size;
	float clique_weight;
	int clique_num;
}node_cliques;

static bool compare_vote_score(const Vote& v1, const Vote& v2) {
	return v1.score > v2.score;
}
static bool compare_vote_degree(const Vote_exp& v1, const Vote_exp& v2) {
	return v1.degree > v2.degree;
}
/*/
int Geometric_consistency(vector<Vote_exp>pts_degree, vector<int>& Match_inlier) {
	for (int i = 0; i < pts_degree.size(); i++)
	{
		Match_inlier.push_back(0);
	}
	int max_degree = pts_degree[0].degree;
	Match_inlier[pts_degree[0].index] = 1;
	for (int i = 0; i < pts_degree[0].degree; i++)
	{
		Match_inlier[pts_degree[0].corre_index[i]] = 1;
	}
	for (int i = 1; i < pts_degree.size(); i++)
	{
		if (pts_degree[i].degree < max_degree)
		{
			break;
		}
		else {
			Match_inlier[pts_degree[i].index] = 1;
			for (int j = 0; j < pts_degree[j].degree; j++)
			{
				Match_inlier[pts_degree[j].corre_index[j]] = 1;
			}
		}
	}
	return 0;
}
*/

//A为待开方的矩阵，N为A矩阵的行数
static Eigen::MatrixXd  MatrixSqrt( const Eigen::MatrixXd & A, int N )
{//只进行开方，不检查矩阵是否可以开方，当矩阵不能开方时，返回的结果会有错误

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> adjoint_eigen_solver((A + A.transpose()) / 2.);//求伴随阵的特征值和特征向量
	
	Eigen::MatrixXd mata = Eigen::MatrixXd::Zero(N, 1);

	return adjoint_eigen_solver.eigenvectors() * adjoint_eigen_solver.eigenvalues().cwiseMax(mata).cwiseSqrt().asDiagonal() * adjoint_eigen_solver.eigenvectors().transpose();

}

inline static float Distance(Eigen::Vector3d& A, Eigen::Vector3d& B) 
{
	float d_x = (float)A[0] - (float)B[0];
	float d_y = (float)A[1] - (float)B[1];
	float d_z = (float)A[2] - (float)B[2];
	float distance = sqrt(d_x * d_x + d_y * d_y + d_z * d_z);
	return distance;
}

static double OTSU_thresh(/*vector<Vote> Vote_score*/Eigen::VectorXd values)
{
	/*vector<double>values;
	for (size_t i = 0; i < Vote_score.size(); i++)
	{
		values.push_back(Vote_score[i].score);
	}*/
	int i;
	int Quant_num = 100;
	double score_sum = 0.0;
	double fore_score_sum = 0.0;
	vector<int> score_Hist(Quant_num, 0);
	vector<double> score_sum_Hist(Quant_num, 0.0);
	double max_score_value, min_score_value;
	vector<double> all_scores;
	for (i = 0; i < values.size(); i++)
	{
		score_sum += values[i];
		all_scores.push_back(values[i]);
	}
	sort(all_scores.begin(), all_scores.end());
	max_score_value = all_scores[all_scores.size() - 1];
	min_score_value = all_scores[0];
	double Quant_step = (max_score_value - min_score_value) / Quant_num;
	for (i = 0; i < values.size(); i++)
	{
		int ID = values[i] / Quant_step;
		if (ID >= Quant_num) ID = Quant_num - 1;
		score_Hist[ID]++;
		score_sum_Hist[ID] += values[i];
	}
	double fmax = -1000;
	int n1 = 0, n2;
	double m1, m2, sb;
	double thresh = (max_score_value - min_score_value) / 2;//default value
	for (i = 0; i < Quant_num; i++)
	{
		double Thresh_temp = i * (max_score_value - min_score_value) / double(Quant_num);
		n1 += score_Hist[i];
		if (n1 == 0) continue;
		n2 = values.size() - n1;
		if (n2 == 0) break;
		fore_score_sum += score_sum_Hist[i];
		m1 = fore_score_sum / n1;
		m2 = (score_sum - fore_score_sum) / n2;
		sb = (double)n1 * (double)n2 * pow(m1 - m2, 2);
		if (sb > fmax)
		{
			fmax = sb;
			thresh = Thresh_temp;
		}
	}
	return thresh;
}

/********yel add end********/

//测量类型（激光雷达和GNSS）
enum MeasurementType { LIDAR, GNSS };
//GNSS状态（RTK固定、RTK浮动和无状态）
//enum GNSSStatus { RTK_FIXED, RTK_FLOAT, NONE };

//封装了传感器测量数据，包括测量类型、时间戳、激光点云数据、IMU数据缓冲区和GNSS位置信息
struct SensorMeasurement {
  MeasurementType measurement_type_;
  // ros time
  double bag_time_{0.0};
  // The time of the first laser point in the scan
  double lidar_start_time_{0.0};
  // The time of the last laser point in the scan
  double lidar_end_time_{0.0};
  CloudPtr cloud_ptr_{};
  std::deque<sensor_msgs::Imu> imu_buff_;

  Eigen::Matrix4d gnss_pose_ = Eigen::Matrix4d::Identity();
  //GNSSStatus gnss_status_ = GNSSStatus::NONE;
  bool has_gnss_ori_{false};
};

class LIO {
 public:

  //存储和管理系统的参数配置，包括初始协方差、重力加速度、迭代次数、点的最大/最小半径
  struct Config {
    Config(){};

    double init_ori_cov{1.0};
    double init_pos_cov{1.0};
    double init_vel_cov{1.0};
    double init_ba_cov{1.0};
    double init_bg_cov{1.0};
    double acc_cov{1.0};
    double gyr_cov{1.0};
    double ba_cov{1.0};
    double bg_cov{1.0};
    //重力加速度
    double gravity{9.81};
    Eigen::Matrix4d T_imu_lidar = Eigen::Matrix4d::Identity();

    size_t max_iterations{30};
    double gicp_constraint_gain{100.0};
    double point2plane_constraint_gain{1000.0};

    bool enable_undistort{true};
    bool enable_outlier_rejection{true};

    double current_scan_resolution{0.5};
    double voxel_map_resolution{0.5};

    double max_radius{150.0};
    double min_radius{0.5};
  };

  //构造函数使用配置参数初始化LIO实例，并设置点云计算的相关指针和其他数据结构。它还设置了重力向量
  LIO(Config config = Config())
      : config_(config)
      , cloud_cov_ptr_(new CloudCovType())
      , cloud_DS_ptr_(new CloudType()) 
      ,filtered_cloud_ptr(new CloudType()){
    fast_voxel_grid_ptr_ =
        std::make_shared<FasterVoxelGrid>(config_.current_scan_resolution);

    VoxelMap::Config voxel_map_config;
    voxel_map_config.resolution = config_.voxel_map_resolution;
    voxel_map_config.search_method = "NEARBY_7";  // nearby_7 is more stable
    voxel_map_ptr_ = std::make_shared<VoxelMap>(voxel_map_config);

    correspondences_array_.reserve(10000);
    //correspondences_array_plane_.reserve(10000);
    g_ = Eigen::Vector3d(0.0, 0.0, config_.gravity);
    my_result_matrix = Eigen::Matrix<double, 8, 6>::Zero();

    down_search_range_.clear();
    for (int z_gain = -1; z_gain >= -10; z_gain--) 
    {
        down_search_range_.emplace_back(Eigen::Vector3d(0,0,z_gain * voxel_map_config.resolution));
    }

  }

  //更新传感器测量数据
  bool MeasurementUpdate(SensorMeasurement& sensor_measurement);
  //根据当前时刻的加速度和角速度预测下一状态
  bool Predict(const double time,
               const Eigen::Vector3d& acc_1,
               const Eigen::Vector3d& gyr_1);
  //静态初始化过程
  bool StaticInitialization(SensorMeasurement& sensor_measurement);
  //姿态和航向参考系统的初始化
  bool AHRSInitialization(SensorMeasurement& sensor_measurement);

  bool InRadius(const PointType& p) {
    double radius = p.x * p.x + p.y * p.y + p.z * p.z;
    return (radius < (config_.max_radius * config_.max_radius) &&
            radius > (config_.min_radius * config_.min_radius));
  }

  bool IsInit() { return lio_init_; }

  //返回当前状态的位姿、速度、加速度偏差和陀螺仪偏差
  Eigen::Matrix4d GetCurrentPose() { return curr_state_.pose; }

  Eigen::Vector3d GetCurrentVel() { return curr_state_.vel; }

  Eigen::Vector3d GetCurrentBa() { return curr_state_.ba; }

  Eigen::Vector3d GetCurrentBg() { return curr_state_.bg; }

  size_t GetFinalIterations() { return iter_num_; }

  void SearchGroups_old();
  void SearchGroups();
  void SearchGroups_V2();

  struct Correspondence {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    Eigen::Vector3d mean_A = Eigen::Vector3d::Zero();
    Eigen::Vector3d mean_B = Eigen::Vector3d::Zero();
    Eigen::Matrix3d mahalanobis = Eigen::Matrix3d::Zero();
    Eigen::Vector4d plane_coeff = Eigen::Vector4d::Zero();
    //! 创新点：增加协方差矩阵的迹
    double tr_A;
    double tr_B;
    double tr_AB;
    double is_valid;
    size_t match_mode;
    size_t current_index;
    std::bitset<27> B_map_features;
    std::bitset<27> A_cur_features;
    bool valid;
    double variance;
    Eigen::Matrix<double, 8, 6> local_result;
    float score;
  };

  tbb::concurrent_vector<std::shared_ptr<Correspondence>> Max_Group_second;

  std::vector<Eigen::Vector3d> down_search_range_;
  //tbb::concurrent_vector<std::shared_ptr<Correspondence>> correspondences_array_plane_;
 private:

  static constexpr int IndexErrorOri{0};
  static constexpr int IndexErrorPos{3};
  static constexpr int IndexErrorVel{6};
  static constexpr int IndexErrorBiasAcc{9};
  static constexpr int IndexErrorBiasGyr{12};

  static constexpr int IndexNoiseAccLast{0};
  static constexpr int IndexNoiseGyrLast{3};
  static constexpr int IndexNoiseAccCurr{6};
  static constexpr int IndexNoiseGyrCurr{9};
  static constexpr int IndexNoiseBiasAcc{12};
  static constexpr int IndexNoiseBiasGyr{15};

  struct PoseHistory {
    double time_ = 0.0;
    Eigen::Matrix4d T_ = Eigen::Matrix4d::Identity();
    Eigen::Vector3d vel_ = Eigen::Vector3d::Zero();
    Eigen::Vector3d un_acc_ = Eigen::Vector3d::Zero();
    Eigen::Vector3d un_gyr_ = Eigen::Vector3d::Zero();
  };
  std::deque<PoseHistory> pose_history_;  // for pointcloud



  struct Index_Group {
    public:
      int current_index;
      Eigen::MatrixXd group;

  };

  std::vector<std::shared_ptr<Correspondence>> correspondences_buff_;
  tbb::concurrent_vector<std::shared_ptr<Correspondence>> correspondences_array_;
  
  tbb::concurrent_vector<Index_Group> Graphs;


  //存储当前位置、速度和偏差（加速度偏差和陀螺仪偏差）
  struct State {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
    Eigen::Vector3d ba = Eigen::Vector3d::Zero();
    Eigen::Vector3d bg = Eigen::Vector3d::Zero();
  };
  //计算给定时间步长下的状态更新，包括位置和速度的变化。它使用传入的加速度和角速度来预测当前的状态。
  bool NominalStateUpdate(const double dt,
                          const Eigen::Vector3d& acc_0,
                          const Eigen::Vector3d& acc_1,
                          const Eigen::Vector3d& gyr_0,
                          const Eigen::Vector3d& gyr_1,
                          const Eigen::Matrix4d& T_prev,
                          const Eigen::Vector3d& vel_prev,
                          Eigen::Matrix4d& T_curr,
                          Eigen::Vector3d& vel_curr,
                          Eigen::Vector3d& un_acc,
                          Eigen::Vector3d& un_gyr);
  //更新机器人姿态估计状态。它是基于非线性ulates框架的，该框架使用广义力矩法（GMF）来估计机器人的姿态
  bool ErrorStateUpdate(const double dt,
                        const Eigen::Vector3d& acc_0,
                        const Eigen::Vector3d& acc_1,
                        const Eigen::Vector3d& gyr_0,
                        const Eigen::Vector3d& gyr_1);
  //对激光雷达点云进行去畸变处理，以便在移动过程中保持点云数据的准确性
  bool UndistortPointCloud(const double bag_time,
                           const double lidar_end_time,
                           CloudPtr& cloud_ptr);
  //执行基于传感器测量数据的优化步骤，输出一个增量（delta_x），该增量可以用来更新状态
  bool StepOptimize(const SensorMeasurement& sensor_measurement,
                    Eigen::Matrix<double, 15, 1>& delta_x);
  //用于构建约束矩阵（H）和向量（b），分别对应于全局点云配准（GICP）、点到平面的约束和IMU先验约束。这些约束被用于优化步骤中，以提高定位精度
  double ConstructGICPConstraints(Eigen::Matrix<double, 15, 15>& H,
                                  Eigen::Matrix<double, 15, 1>& b);

  //用于构建约束矩阵（H）和向量（b），分别对应于全局点云配准（使用Wasserstein距离的GICP）、点到平面的约束和IMU先验约束。这些约束被用于优化步骤中，以提高定位精度
  double ConstructWassersteinGICPConstraints(Eigen::Matrix<double, 15, 15>& H,
                                  Eigen::Matrix<double, 15, 1>& b);

  double ConstructPoint2PlaneConstraints(Eigen::Matrix<double, 15, 15>& H,
                                         Eigen::Matrix<double, 15, 1>& b);

  double ConstructImuPriorConstraints(Eigen::Matrix<double, 15, 15>& H,
                                      Eigen::Matrix<double, 15, 1>& b);

  double ConstructGravityConstraints(Eigen::Matrix<double, 15, 15>& H,
                                      Eigen::Matrix<double, 15, 1>& b);

  //根据传感器测量和当前状态估算的误差，误差是优化算法的关键
  double ComputeError(const SensorMeasurement& sensor_measurement,
                      const State& state);
  //检查优化过程是否收敛，通常通过一定的阈值来判断增量是否足够小
  bool IsConverged(const Eigen::Matrix<double, 15, 1>& delta_x);
  //计算最终的协方差矩阵，以判断状态的不确定性
  bool ComputeFinalCovariance(const Eigen::Matrix<double, 15, 1>& delta_x);
  //根据当前状态和计算出的增量更新状态。这是估计过程的一个核心步骤
  bool CorrectState(const State& state,
                    const Eigen::Matrix<double, 15, 1>& delta_x,
                    State& corrected_state);

  bool GNStep(const SensorMeasurement& sensor_measurement,
              Eigen::Matrix<double, 15, 15>& H,
              Eigen::Matrix<double, 15, 1>& b,
              const double y0,
              Eigen::Matrix<double, 15, 1>& delta_x);
  void Graph_construction(tbb::concurrent_vector<std::shared_ptr<Correspondence>>& correspondence, float resolution, bool sc2, float cmp_thresh,float alpha_dis_temp); 
  void Graph_construction_V2(tbb::concurrent_vector<std::shared_ptr<Correspondence>>& correspondence, float resolution, bool sc2, float cmp_thresh,float alpha_dis_temp); 
  
  Eigen::MatrixXd Graph_construction_old(tbb::concurrent_vector<std::shared_ptr<Correspondence>>& correspondence, float resolution, bool sc2, float cmp_thresh, float alpha_dis_temp);

  //void Graph_construction_old(tbb::concurrent_vector<std::shared_ptr<Correspondence>>& correspondence, float resolution, bool sc2, float cmp_thresh,float alpha_dis_temp); 
  Config config_;

  double lio_time_{0.0};
  bool lio_init_{false};

  Eigen::Vector3d g_ = Eigen::Vector3d(0.0, 0.0, 9.81);

  CloudPtr filtered_cloud_ptr;
  State curr_state_;    
  State prev_state_;

  Eigen::Vector3d acc_0_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d gyr_0_ = Eigen::Vector3d::Zero();

  Eigen::Matrix<double, 15, 15> P_ = Eigen::Matrix<double, 15, 15>::Zero();
  Eigen::Matrix<double, 15, 15> F_ = Eigen::Matrix<double, 15, 15>::Zero();
  Eigen::Matrix<double, 15, 18> B_ = Eigen::Matrix<double, 15, 18>::Zero();
  Eigen::Matrix<double, 18, 18> Q_ = Eigen::Matrix<double, 18, 18>::Zero();

Eigen::Matrix<double, 8, 6> my_result_matrix;

  double transformation_epsilon_{0.001};
  bool need_converge_{false};
  size_t iter_num_{0};
  Eigen::Matrix<double, 15, 15> final_hessian_ =
      Eigen::Matrix<double, 15, 15>::Zero();

  size_t effect_feat_num_;
  CloudCovPtr cloud_cov_ptr_{};
  CloudPtr cloud_DS_ptr_{};
  std::shared_ptr<VoxelMap> voxel_map_ptr_{};
  std::shared_ptr<FasterVoxelGrid> fast_voxel_grid_ptr_{};
  std::shared_ptr<std::map<size_t,size_t>> index_AB;
  size_t lidar_frame_count_{0};
  size_t keyframe_count_{0};

  // for initialization
  Eigen::Vector3d mean_acc_ = Eigen::Vector3d(0, 0, -1.0);
  Eigen::Vector3d mean_gyr_ = Eigen::Vector3d(0, 0, 0);
  // <acc, gyr>
  std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> imu_init_buff_;
  bool first_imu_frame_{true};
  size_t imu_init_count_{0};
  size_t max_init_count_{20};

  Eigen::Matrix4d last_keyframe_pose_ = Eigen::Matrix4d::Identity();

  size_t effect_const_num_{0};
  double ava_effect_feat_num_{0.0};

void find_largest_clique_of_node(const Eigen::MatrixXd& Graph, igraph_vector_int_list_t* cliques, node_cliques* result, vector<int>& remain, int num_node, int est_num, string descriptor) {
    int* vis = new int[igraph_vector_int_list_size(cliques)];
    memset(vis, 0, igraph_vector_int_list_size(cliques));
#pragma omp parallel for
    for (int i = 0; i < num_node; i++)
    {
        result[i].clique_index = -1;
        result[i].clique_size = 0;
        result[i].clique_weight = 0;
        result[i].clique_num = 0;
    }

    for (int i = 0; i < remain.size(); i++)
    {
        igraph_vector_int_t* v = igraph_vector_int_list_get_ptr(cliques, remain[i]);
        float weight = 0;
        int length = igraph_vector_int_size(v);
        for (int j = 0; j < length; j++)
        {
            int a = (int)VECTOR(*v)[j];
            for (int k = j + 1; k < length; k++)
            {
                int b = (int)VECTOR(*v)[k];
                weight += Graph(a, b);
            }
        }
        for (int j = 0; j < length; j++)
        {
            int k = (int)VECTOR(*v)[j];
            if (result[k].clique_weight < weight)
            {
                result[k].clique_index = remain[i];
                vis[remain[i]]++;
                result[k].clique_size = length;
                result[k].clique_weight = weight;
            }
        }
    }

#pragma omp parallel for
    for (int i = 0; i < remain.size(); i++)
    {
        if (vis[remain[i]] == 0) {
            igraph_vector_int_t* v = igraph_vector_int_list_get_ptr(cliques, remain[i]);
            igraph_vector_int_destroy(v);
        }
    }

    vector<int>after_delete;
    for (int i = 0; i < num_node; i++)
    {
        if (result[i].clique_index < 0)
        {
            continue;
        }
        if (vis[result[i].clique_index] > 0)
        {
            vis[result[i].clique_index] = 0;
            after_delete.push_back(result[i].clique_index);
        }
        else if (vis[result[i].clique_index] == 0) {
            result[i].clique_index = -1;
        }
    }
    remain.clear();
    remain = after_delete;
    if (remain.size() > est_num)
    {
        vector<int>after_decline;
        vector<Vote>clique_score;
        for (int i = 0; i < num_node; i++)
        {
            if (result[i].clique_index < 0)
            {
                continue;
            }
            Vote t;
            t.index = result[i].clique_index;
            t.score = result[i].clique_weight;
            clique_score.push_back(t);
        }
        sort(clique_score.begin(), clique_score.end(), compare_vote_score);
        for (int i = 0; i < est_num; i++)
        {
            after_decline.push_back(clique_score[i].index);
        }
        remain.clear();
        remain = after_decline;
        clique_score.clear();
    }
    delete[] vis;
    return;
}

};

#endif